/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package br.ufrn.dca.scontrol.control;

/**
 *
 * @author marco
 */
public class Controladores {

    private double Kp;
    private double Ki;
    private double Kd;
    private double integral;
    private double erro;
    private double amostragem;
    private boolean antiWindup;
    private double Tt;
    private double Td;
    private boolean filtroDerivativo;

    public double getTd() {
        return Td;
    }

    public void setTd(double Td) {
        this.Td = Td;
    }

    public double getTt() {
        return Tt;
    }

    public void setTt(double Tt) {
        this.Tt = Tt;
    }

    public boolean isFiltroDerivativo() {
        return filtroDerivativo;
    }

    public void setFiltroDerivativo(boolean filtroDerivativo) {
        this.filtroDerivativo = filtroDerivativo;
    }

    public boolean isAntiWindup() {
        return antiWindup;
    }

    public void setAntiWindup(boolean antiWindup) {
        this.antiWindup = antiWindup;
    }

    public Controladores() {
        Kp = 0;
        Ki = 0;
        Kd = 0;
        integral = 0;
        erro = 0;
        amostragem = 0.1;
    }

    public void setKp(double Kp) {
        this.Kp = Kp;
    }

    public void setKi(double Ki) {
        this.Ki = Ki;
    }

    public void setKd(double Kd) {
        this.Kd = Kd;
    }

    public double getKp() {
        return Kp;
    }

    public double getKi() {
        return Ki;
    }

    public double getKd() {
        return Kd;
    }

    /*
     * Definindo Ações Proporcional, Integrativa e DerivativA
     */
    public double acaoProporcional(double e) {
        double P = (Kp * e);
        return P;
    }

    public double acaoIntegrativa(double e) {
        double I = integral + (Ki * amostragem * e);
        if (!isAntiWindup()) {
            if(I <= 3 && I >= -3)
                integral = I;
        }
        else{
            integral = I;
        }
        return I;
    }

    public double acaoDerivativa(double e) {
        double D = Kd * (e - erro) / amostragem;
        erro = e;
        return D;
    }

    public double controladorP(double e) {
        return acaoProporcional(e);
    }

    public double controladorPI(double e) {
        double actionSat;
        double action;
        double actionAntiWindup;
        action = acaoProporcional(e) + acaoIntegrativa(e);

//        actionSat = action;

//        if (isAntiWindup()) {
//            if (action > (Double) 3.00) {
//                actionSat = 3.00;
//            }
//            if (action < -3.00) {
//                actionSat = -3.00;
//            }
//            actionAntiWindup = (actionSat - action);
//            actionAntiWindup *= (1 / this.Tt);
//            actionAntiWindup += this.Ki*e*amostragem;
//            actionAntiWindup += integral;
//
//            action = actionAntiWindup + acaoProporcional(e);
//            integral = actionAntiWindup;
//
//
//        }
        return action;
    }

    public double controladorPD(double e) {
        return acaoProporcional(e) + acaoDerivativa(e);
    }

    public double controladorPID(double e) {
        double actionSat;
        double action;
        double actionAntiWindup;
        action = acaoProporcional(e) + acaoIntegrativa(e) + acaoDerivativa(e);
        actionSat = action;

//        if (isAntiWindup()) {
//            if (action > (Double) 3.00) {
//                actionSat = 3.00;
//            }
//            if (action < -3.00) {
//                actionSat = -3.00;
//            }
//            actionAntiWindup = (actionSat - action);
//            actionAntiWindup *= (1 / this.Tt);
//            actionAntiWindup += this.Ki*e*amostragem;
//            actionAntiWindup += integral;
//
//            action = actionAntiWindup + acaoProporcional(e) + acaoDerivativa(e);
//            integral = actionAntiWindup;
//        }
        return action;
    }

    public double controladorPI_D(double e, double c) {
        double actionSat;
        double action;
        double actionAntiWindup;
        action = acaoProporcional(e) + acaoIntegrativa(e) + acaoDerivativa(c);
        actionSat = action;

        if (isAntiWindup()) {
            if (action > (Double) 3.00) {
                actionSat = 3.00;
            }
            if (action < -3.00) {
                actionSat = -3.00;
            }
            //System.out.println("Tt: " + Tt + " action: " + action + " actionSat: " + actionSat);
            actionAntiWindup = (actionSat - action);
            //System.out.println("actWindUp 1: " + actionAntiWindup);
            actionAntiWindup *= (1 / this.Tt);
            actionAntiWindup += this.Ki*e*amostragem;
            actionAntiWindup += integral;

            action = actionAntiWindup + acaoProporcional(e) + acaoDerivativa(c);
            integral = actionAntiWindup;
            
            //System.out.println("actWindUp 2: " + actionAntiWindup);
        }
        return action;
    }
}
